/*
 * TSK_AHRS.c
 *
 *  Created on: 2013-02-19
 *      Author: Miles

 *	AHRS(attitude and heading reference system)
 *
 *
 *
 *
 */

#include "funcdef.h"


//extern double imu_info[14];
//extern double timelabel_pre;

void ahrs_tsk(void)
{
	double att_pre[3],att_est[3],q_pre[4];
	double ahrs_gyro_bias[3];
	double ahrs_xk_pre[7], Pk_pre[49], ahrs_xk[7], Pk[49];
	double eINT[3] = {0};
	double Kp, Ki, Kd;
	Kp = 6.5;
	Ki = 0.0011;
	Kd = 0;

	MatZero(7, 1, ahrs_xk_pre);
	MatZero(7, 7, Pk_pre);
	MatZero(7, 1, ahrs_xk);
	MatZero(7, 7, Pk);

	Pk_pre[0] = 0.002 * 0.002;
	Pk_pre[8] = 0.0083 * 0.0083;
	Pk_pre[16] = 0.0091 * 0.0091;
	Pk_pre[24] = 0.0435 * 0.0435;
	Pk_pre[32] = pow((5.95 / 3600 * D2R),2);
	Pk_pre[40] = pow((25.27 / 3600 * D2R),2);
	Pk_pre[48] = pow((6.19 / 3600 * D2R),2);

	while(AHRS_INIT(ahrs_gyro_bias, att_est) == 0) {};
	//Add LED green on here
	Att2Qbn(att_est, q_pre);
	ahrs_xk_pre[0] = q_pre[0];
	ahrs_xk_pre[1] = q_pre[1];
	ahrs_xk_pre[2] = q_pre[2];
	ahrs_xk_pre[3] = q_pre[3];


	while(1)
	{
		//while(get_imu_info() == 0)	{};	//wait here until getting the imu_info and save it to imu_info[14]
		timelabel_pre = AHRS_EKF_UPDATE(timelabel_pre, imu_info, ahrs_xk_pre, ahrs_gyro_bias, Pk_pre, ahrs_xk, Pk, AC, att_est);
		//add output 'q'(ahrs_xk[0-3]) or 'att_est(rad)' here to UART
		MatEquald(7, 1, ahrs_xk, ahrs_xk_pre);	//this sentence can add into AHRS.c
		MatEquald(7, 7, Pk, Pk_pre);	//this sentence can add into AHRS.c

	}
}
